pis_robotics_control_systemfandomcom-20200215-history
Full example of spring-mass-damper system
This a complete tutorial of how to model and simulate a spring-mass-damper system using MATLAB. This tutorial will cover the following topics. # Derivation of differential equation # Converting to State-space representation # Implementing with MATLAB Assume that the system is described as follows. * A block with mass of 1 kg is attached to one end of a spring with the spring constant of 10 N/m * The other end of the spring is attached to a fixed wall. * The equilibrium position of the spring is 6 m from the wall. * The block travels along the floor that is perpendicular to the wall. * The friction between the block and the floor is linearly proportional to the speed of the block. * The damping coefficient of the block is 0.5 kg/s. Top View of Spring-Mass-Damper System 3D View of Spring-Mass-Damper System Derivation of differential equation Assume that there is only horizontal motion, which perpendicular to the wall. The dynamics of the system can be derived based on forces acting on the block. There are two forces acting on the block, assuming that there is no gravity. THe first one is the spring force. We know that the magnitude of the spring force is linearly proportional to the change in displacement from equilibrium position. Therefore, we can express the spring force as the following. F_s=-k(x-x_e) (1) where F_s is the spring force in N k is the spring constant in \frac{N}{m} x is the total displacement of the block from the wall in m x_e is the position of the block when the spring is at equilibrium point in m We also know that the friction can be modeled as a linear drag force. The magnitude of the drag force is linearly proportional to the speed of the motion. Similar to the spring force, the direction of drag force is the opposition to the motion. We can express the drag force as the following. F_d=-b\dot{x} (2) where F_d is the drag force in N b is the damping coefficient in \frac{kg}{s} Therefore, sum of all forces acting on the block can be expressed as the summation of two forces. \sum\limits F = F_d + F_s (3) According the Newton's 2nd law, sum of all forces acting on the object is equal to rate of change of linear momentum p in an inertial reference frame. Sum of the force can be expressed as the following. \sum\limits F = \frac{d}{dt}(p) = \frac{d}{dt}(m\dot{x})= m\ddot{x} (4) By combining and arranging equations (3) and (4), we obtain the second-order differential equation that represent the dynamics of the block. m\ddot{x}+b\dot{x}+k(x-x_e) = 0 (5) This is a second-order linear time-invariant system with a forcing function of a constant due to the equilibrium position. Converting to State-space representation Since this is a second-order differential equation, there must only be 2 state variables. The change in first state variable can be described by \dot{x} . Therefore, x is one of the state variables. \ddot{x} also represents a change in \dot{x} . Hence, \dot{x} is the other state variable. Writing State Equation Let the first and second state variables denoted by x_1 and x_2 respectively. By assigning the state variables correctly, the state of the system can be described by the following equation. x_1 = x x_2 = \dot{x} By differentiating both equations, two first-order differential equations are obtained. \dot{x_1} = \dot{x} (6) \dot{x_2} = \ddot{x} Since \dot{x} is assigned to x_2 , \dot{x} in equation (6) can be replaced by x_2 . \ddot{x} can be substituted by given second-order differential equation (5). \dot{x_2} = \frac{1}{m}(-b\dot{x}-k(x-x_e)) (7) Notice that variable x can be replaced by state variable x_1 . Therefore, the state equation can be written as \dot{x_1} = x_2 (8) \dot{x_2} = \frac{1}{m}(-bx_2-k(x_1-x_e)) (9) Writing Output Equation For the sake of completeness, let the output of the sensor be equal to state variable the state variable x . y_1 = x Since x can be replaced by x_1 , the output equation can be expressed as follows. y_1 = x_1 Implementation with MATLAB Now that we have the state-space representation of the system, all we have to do is to specify some constraints for simulation. Setting Up Parameters and Initial Condition These are constraints for the simulation of the pendulum. * The initial time is zero. The duration of simulation is 10 seconds. * The block is initially pulled 1 m from the wall and released from rest. * The time step is variable. Time Elements In the code, we will use "startTime" for start time. We will also use duration to describe how long that the simulation will last. We will also denote the duration of the simulation by "duration".' '''The '''time step' can be either fixed or variable. Therefore, there is no need to define the sample rate. The time vector will be generated by ode solver based on the given information. The following is the sample code in MATLAB. startTime = 0; duration = 10; Physical Parameters There are only 4 parameters in the system, mass, damping coefficient, spring constant, and the equilibrium position. The following is the sample code in MATLAB for declaring physical parameters of the system. m = 1; b = 0.5; k = 10; x_e = 5; Initial Conditions We will store the initial conditions in a row vector. The first initial condition is the initial displacement of 7 m. The last initial condition is zero because the motion starts at rest. The following is the sample code in MATLAB for assigning the initial conditions of the system. We will denote the initial condition by "IC". IC = [ 7 , 0 ]; Defining the dynamics of the system We will use anonymous function to describe the dynamics. Because we are using ODE solver in MATLAB, the change in state vector have to be assigned as a column vector. Inside the function handle, we will write the rate of change of each state variables. The following is the sample code in MATLAB for describing the dynamics of pendulum using anonymous function. dynamics = @(t,x)(; (-k*(x(1)-x_e)-b*x(2))/m) ; Notice: x refers to a state vector. Therefore, in order to access x_1 , we write x(1) to access the first element in the vector x. Using ODE solver to find Solutions We will use ode45 to solve our system. As an overview for ode45, this function will take a function handle that describes the differential equations as well as time vector and initial conditions. The function will compute the state variables that corresponds to each time stamp as well as the time vector.Since we are using variable time step, the time vector can be described as an array of two elements that contains start time and end time. The following is the sample code in MATLAB for generating solution of state variables using ode45. t,X = ode45(dynamics,startTime+duration,IC); Hence variable t in the code referred to generated time vector. Interpret the Solutions with Graph and Animation To prevent confusion, we will use "x_1" and "x_2" for the displacement and velocity at all given time stamp , respectively. Trajectory Graph (against time) One can plot the both result against time to see how the pendulum behaves as the time progresses. We will use the function "plot" for displaying a graph. The following is the sample code in MATLAB for generating a plot with solutions from ode45. x_1 = X(:,1); x_2 = X(:,2); figure(1) % start a new figure #1 hold on; % hold for multiple plots plot(t,x_1,'b'); plot(t,x_2,'r'); legend('x_1','x_2'); xlabel('t s'); ylabel('x_1,x_2'); title('x vs. t'); The entire script can be written as follows. startTime = 0; duration = 10; m = 1; b = 0.5; k = 10; x_e = 5; IC = [ 7 , 0 ]; dynamics = @(t,x)(; (-k*(x(1)-x_e)-b*x(2))/m) ; t,X = ode45(dynamics,startTime+duration,IC); x_1 = X(:,1); x_2 = X(:,2); figure(1) hold on; plot(t,x_1,'b'); plot(t,x_2,'r'); legend('x_1','x_2'); xlabel('t s'); ylabel('x_1,x_2'); title('x vs. t'); Animation (in Cartesian Space) We will not go in the detail since this requires more experience. The example code is given as a function that will display the trajectory as well as 3D animation of a block. function drawSMD startTime = 0; duration = 10; m = 1; b = 0.5; k = 10; x_e = 5; IC = , 0; dynamics = @(t,x)(x(2);(-b*x(2)-k*(x(1)-x_e))/m); t,X = ode45(dynamics,startTime+duration,IC); x_1 = X(:,1); x_2 = X(:,2); figure(1) subplot(1,2,1); hold on; plot(t,x_1,'b'); plot(t,x_2,'r'); legend('x_1','x_2'); xlabel('t s'); ylabel('x_1,x_2'); title('x vs. t'); subplot(1,2,2) hold on; H = 2; W = 3; L = 4; x = x_1(1); y = 0; z = H/2; p1 = drawbox(x,y,z,L,W,H,0); sW = 0.5; n = 5; w = 2*pi*n/(x-L/2); T = linspace(0,(x-L/2),1000); xs = T; ys = sW*cos(w*xs)+y; zs = sW*sin(w*xs)+z; p2 = drawbox(-0.5,0,3,1,8,6,200); p3 = plot3(xs,ys,zs,'LineWidth',4,'Color','k'); axis(11 -6 6 0 10) axis equal; angle1 = 30; angle2 = 30; view(angle1,angle2); step = 2; for i = 1+step:step:length(t), length(t) x = x_1(i); vx1 =y-W/2 z-H/2; vx2 =y-W/2 z-H/2; vx3 =y+W/2 z-H/2; vx4 =y+W/2 z-H/2; vx5 =y-W/2 z+H/2; vx6 =y-W/2 z+H/2; vx7 =y+W/2 z+H/2; vx8 =y+W/2 z+H/2; Xdata = [ vx1(1) vx5(1) vx1(1) vx4(1) vx1(1) vx2(1);... vx2(1) vx6(1) vx2(1) vx3(1) vx4(1) vx3(1);... vx3(1) vx7(1) vx6(1) vx7(1) vx8(1) vx7(1);... vx4(1) vx8(1) vx5(1) vx8(1) vx5(1) vx6(1)]; set(p1, 'XData', Xdata ); w = 2*pi*n/(x-L/2); T = linspace(0,(x-L/2),1000); xs = T; ys = sW*cos(w*xs)+y; zs = sW*sin(w*xs)+z; set(p3,'XData',xs,'YData',ys,'ZData',zs); view(angle1+i,angle2); pause(t(i)-t(i-step)); end function p = drawbox(x,y,z,L,W,H,c) v1 =y-W/2 z-H/2; v2 =y-W/2 z-H/2; v3 =y+W/2 z-H/2; v4 =y+W/2 z-H/2; v5 =y-W/2 z+H/2; v6 =y-W/2 z+H/2; v7 =y+W/2 z+H/2; v8 =y+W/2 z+H/2; xdata = [ v1(1) v5(1) v1(1) v4(1) v1(1) v2(1);... v2(1) v6(1) v2(1) v3(1) v4(1) v3(1);... v3(1) v7(1) v6(1) v7(1) v8(1) v7(1);... v4(1) v8(1) v5(1) v8(1) v5(1) v6(1)]; ydata = [ v1(2) v5(2) v1(2) v4(2) v1(2) v2(2);... v2(2) v6(2) v2(2) v3(2) v4(2) v3(2);... v3(2) v7(2) v6(2) v7(2) v8(2) v7(2);... v4(2) v8(2) v5(2) v8(2) v5(2) v6(2)]; zdata = [ v1(3) v5(3) v1(3) v4(3) v1(3) v2(3);... v2(3) v6(3) v2(3) v3(3) v4(3) v3(3);... v3(3) v7(3) v6(3) v7(3) v8(3) v7(3);... v4(3) v8(3) v5(3) v8(3) v5(3) v6(3)]; figure(1) hold on; p = patch(xdata,ydata,zdata,'w'); set(gca,'CLim',40) cdata = c'; set(p,'FaceColor','flat',... 'FaceVertexCData',cdata,... 'CDataMapping','scaled') end end